Three-dimensional measurement apparatus, processing method, and non-transitory computer-readable storage medium

ABSTRACT

A three-dimensional measurement apparatus selects points corresponding to geometric features of a three-dimensional shape model of a target object, projects a plurality of selected points corresponding to the geometric features onto a range image based on approximate values indicating the position and orientation of the target object and imaging parameters at the time of imaging of the range image, searches regions of predetermined ranges respectively from the plurality of projected points for geometric features on the range image which correspond to the geometric features of the three-dimensional shape model, and associates these geometric features with each other. The apparatus then calculates the position and orientation of the target object using differences of distances on a three-dimensional space between the geometric features of the three-dimensional shape model and those on the range image, which are associated with each other.

TECHNICAL FIELD

The present invention relates to a three-dimensional measurementapparatus, processing method, and non-transitory computer-readablestorage medium.

BACKGROUND ART

In recent years, along with the growth of robot technologies, robotsperform complicated tasks (for example, assembling processes ofindustrial products), which were, up until now, performed manually. Suchrobots assemble parts by gripping them using an end effecter such as ahand.

In order to control the robot to grip a part, a relative position andorientation between the part to be gripped and the robot (hand) have tobe measured (estimated). Such position and orientation measurements areused not only when the robot grips a part but also in various purposessuch as self position estimation required for the robot to autonomouslymove and registration between a real space and virtual object inaugmented reality.

In the position and orientation measurements, methods using atwo-dimensional image captured by a camera and a range image obtainedfrom a range sensor are known. For example, measurements by means ofmodel fitting are known. In the measurements, a three-dimensional shapemodel of an object is fitted to features detected from thetwo-dimensional image or to the range image.

In the model fitting to a two-dimensional image, the position andorientation are measured by fitting a projected image obtained byprojecting a three-dimensional shape model onto an image to featuresdetected from a two-dimensional image. In the model fitting to a rangeimage, respective points of the range image are converted into athree-dimensional point group having three-dimensional coordinates, anda three-dimensional shape model is fitted to the three-dimensional pointgroup on a three-dimensional space, thereby measuring the position andorientation.

As a method of measuring the position and orientation using atwo-dimensional image, a method of measuring the position andorientation of a camera using edges is known (T. Drummond and R.Cipolla, “Real-time visual tracking of complex structures,” IEEETransactions on Pattern Analysis and Machine Intelligence, vol. 24, no.7, pp. 932-946, 2002. (to be referred to as reference 1 hereinafter)).With this method, a three-dimensional shape of an object is expressed bya set of line segments (wire frame model), and projected images ofthree-dimensional line segments are fitted to edges detected on animage, thereby measuring the position and orientation of the object.More specifically, three-dimensional line segments are projected ontothe image based on approximate values of the position and orientation,so as to detect edges in the vicinity of the projected line segments.Next, the position and orientation of a target object are measured bynonlinear optimization, so as to minimize a sum total of distances onthe image between the projected images of the line segments based on theapproximate values of the position and orientation, and thecorresponding edges.

On the other hand, as a method of measuring the position and orientationusing a range image, a method using an ICP (Iterative Closest Point)algorithm is known (P. J. Besl and N. D. McKay, “A method forregistration of 3-D shapes,” IEEE Transactions on Pattern Analysis andMachine Intelligence, vol. 14, no. 2, pp. 239-256, 1992. (to be referredto as reference 2 hereinafter)). With this method, the position andorientation of an object are measured by fitting a three-dimensionalshape model of the object to three-dimensional point group dataconverted from a range image. Processing for searching geometricfeatures of the three-dimensional shape model closest tothree-dimensional points based on approximate values of the position andorientation, and updating the position and orientation so as to minimizea sum total of distances between points and the geometric features ofthe three-dimensional model is iteratively executed.

The method of measuring the position and orientation of an object usinga range image requires heavy-load arithmetic processing upon searchingfor planes of the three-dimensional shape model corresponding torespective points of the point group data. In order to cope with this,Japanese Patent Laid-Open No. 2006-202152 (to be referred to asreference 3 hereinafter) discloses a technique that speeds upassociation search processing at the time of registration of a pluralityof range images. In this method, small planes (meshes) assigned withindex values are fitted to a range image, the index values are convertedinto non-overlapping colors to obtain colors of the meshes, and an indeximage is generated by rendering the meshes based on the imaging positionand orientation of the range image. Then, a three-dimensional pointgroup converted from another range image is projected onto the indeximage based on the position and orientation at the time of imaging, andcorresponding colors on the index image are acquired from the coordinatevalues of the projected three-dimensional points. After that, the indexvalues corresponding to the acquired colors are inversely converted toassociate the meshes with the three-dimensional points converted fromthe range image. In this manner, the association search processing isspeeded up.

In the aforementioned registration method (reference 3) using the indeximage, in order to register the positions of a plurality of rangeimages, all three-dimensional points extracted from the range images areprojected onto a two-dimensional image to associate them with eachother. For this reason, for example, when the position and orientationof a target object are measured from an image including an object otherthan the target object, since all measurement points including those ofthe object other than the target object have to be densely projected,resulting in wasteful processing.

In case of the method of reference 3, depending on three-dimensionalshape models, the model has to be re-meshed so as to prevent meshes frombeing crushed at the time of projection. For this reason, it isdifficult to use a CAD model of a target object intact as athree-dimensional shape model. Furthermore, in order to render meshes ona two-dimensional plane at high speed, dedicated hardware such as a GPU(Graphic Processing Unit) is required.

Furthermore, in case of the method of reference 1, all line segments asgeometric features of a three-dimensional shape model as well as thoseon a back surface (portion which is not measured) of a target objecthave to be projected onto an image. Hence, the method of reference 1also requires wasteful processing.

In this case, the method using a two-dimensional image is suited to, forexample, an environment including many artificial objects on the premiseof lines, and the method using a range image is suited to, for example,an object having a plurality of smooth planes.

Since the method using a two-dimensional image and that using a rangeimage require different properties of information to be measured, theposition and orientation measurement precision is expected to beimproved by combining the model fitting to a two-dimensional image andthat to a range image. In the model fitting to a two-dimensional image,processing for projecting geometric features of a three-dimensionalshape model such as edges onto a two-dimensional image, and searchingfor corresponding geometric features on the two-dimensional image isexecuted, as described above. In the model fitting to a range image,measurement points are projected in place of geometric features of athree-dimensional shape model, as described above. That is, thesemethods use different association methods.

For this reason, the association processing cannot be executed using anidentical framework, and has to be executed independently upon measuringthe position and orientation of a target object using both atwo-dimensional image and range image.

SUMMARY OF INVENTION

The present invention provides a technique which allows to speed upposition and orientation calculations of a target object by efficientlymaking arithmetic operations executed upon associating athree-dimensional shape and measurement data with each other.

According to a first aspect of the present invention, there is provideda three-dimensional measurement apparatus comprising; image acquisitionmeans for acquiring a range image in which pixels represent distancesfrom a target object; approximate value acquisition means for acquiringapproximate values indicating a position and an orientation of thetarget object; selection means for selecting points corresponding togeometric features of a three-dimensional shape model of the targetobject; projection means for projecting a plurality of pointscorresponding to the geometric features selected by the selection meansonto the range image based on the approximate values indicating theposition and the orientation of the target object and imaging parametersat the time of imaging of the range image, search means for searchingregions of predetermined ranges respectively from the plurality ofprojected points for geometric features on the range image whichcorrespond to the geometric features of the three-dimensional shapemodel, and associating the geometric features with each other; andposition/orientation calculation means for calculating a position and anorientation of the target object using differences of distances on athree-dimensional space between the geometric features of thethree-dimensional shape model and the geometric features on the rangeimage, which are associated with each other by the search means.

According to a second aspect of the present invention, there is provideda processing method for a three-dimensional measurement apparatus forcalculating a position and an orientation of a target object,characterized by comprising: a step of acquiring a range image in whichpixels represent distances from a target object; a step of acquiringapproximate values indicating a position and an orientation of thetarget object; a step of selecting points corresponding to geometricfeatures of a three-dimensional shape model of the target object; a stepof projecting a plurality of selected points corresponding to thegeometric features onto the range image based on the approximate valuesindicating the position and the orientation of the target object andimaging parameters at the time of imaging of the range image; a step ofsearching regions of predetermined ranges respectively from theplurality of projected points for geometric features on the range imagewhich correspond to the geometric features of the three-dimensionalshape model, and associating the geometric features with each other; anda step of calculating a position and an orientation of the target objectusing differences of distances on a three-dimensional space between thegeometric features of the three-dimensional shape model and thegeometric features on the range image, which are associated with eachother.

According to a third aspect of the present invention, there is provideda non-transitory computer-readable storage medium storing a computerprogram for controlling a computer incorporated in a three-dimensionalmeasurement apparatus for calculating a position and an orientation of atarget object, to function as: image acquisition means for acquiring arange image in which pixels represent distances from a target object;approximate value acquisition means for acquiring approximate valuesindicating a position and an orientation of the target object; selectionmeans for selecting points corresponding to geometric features of athree-dimensional shape model of the target object; projection means forprojecting a plurality of points corresponding to the geometric featuresselected by the selection means onto the range image based on theapproximate values indicating the position and the orientation of thetarget object and imaging parameters at the time of imaging of the rangeimage, search means for searching regions of predetermined rangesrespectively from the plurality of projected points for geometricfeatures on the range image which correspond to the geometric featuresof the three-dimensional shape model, and associating the geometricfeatures with each other; and position/orientation calculation means forcalculating a position and an orientation of the target object usingdifferences of distances on a three-dimensional space between thegeometric features of the three-dimensional shape model and thegeometric features on the range image, which are associated with eachother by the search means.

Further features of the present invention will be apparent from thefollowing description of exemplary embodiments (with reference to theattached drawings).

BRIEF DESCRIPTION OF DRAWINGS

The accompanying drawings, which are incorporated in and constitute apart of the specification, illustrate embodiments of the invention, andtogether with the description, serve to explain the principles of theinvention.

FIG. 1 is a block diagram showing an example of the arrangement of athree-dimensional measurement apparatus according to an embodiment ofthe present invention;

FIG. 2 is a view showing an example of association processing by anassociation search unit 150 shown in FIG. 1;

FIG. 3 is a view showing an example of association processing by theassociation search unit 150 shown in FIG. 1;

FIG. 4 is a flowchart showing an example of the operation in athree-dimensional measurement apparatus 10 shown in FIG. 1; and

FIG. 5 is a view showing an example of association processing accordingto the second embodiment.

DESCRIPTION OF EMBODIMENTS

An exemplary embodiment(s) of the present invention will now bedescribed in detail with reference to the drawings. It should be notedthat the relative arrangement of the components, the numericalexpressions and numerical values set forth in these embodiments do notlimit the scope of the present invention unless it is specificallystated otherwise.

First Embodiment

FIG. 1 is a block diagram showing an example of the arrangement of athree-dimensional measurement apparatus according to an embodiment ofthe present invention. Note that a three-dimensional measurementapparatus 10 incorporates a computer. The computer includes a maincontrol unit such as a CPU, and storage units such as a ROM (Read OnlyMemory), RAM (Random Access Memory), and HDD (Hard Disk Drive). Thecomputer may also include input/output units such as buttons and adisplay or touch panel, and a communication unit such as a network card.Note that these components are connected via, for example, a bus, andare controlled when the main control unit executes programs stored inthe storage units.

The three-dimensional measurement apparatus 10 projects (arbitrary)points on geometric features of a three-dimensional shape model onto atwo-dimensional plane, and searches for geometric features on thetwo-dimensional plane (range image), which exist within regions ofpredetermined ranges from the projected points on the two-dimensionalplane. Then, the geometric features of the three-dimensional shape modelare associated with those on the range image, and the position andorientation of a target object are calculated based on the associatedgeometric features.

The three-dimensional measurement apparatus 10 is configured to includean image acquisition unit 110, approximate value acquisition unit 120,model holding unit 130, geometric feature selection unit 140,association search unit 150, and position/orientation calculation unit160.

The image acquisition unit 110 acquires a range image. In thisembodiment, the image acquisition unit 110 is implemented by, forexample, a range sensor which captures a range image. However, thepresent invention is not limited to this. For example, the imageacquisition unit 110 may adopt an arrangement for simply acquiring arange image captured by an external range sensor. The range image is animage in which each pixel has depth information (distance information).The range sensor can adopt an active system that captures reflectedlight of laser light or slit light with which a target is irradiated,and measures a distance by triangulation. Note that the range sensorneed only capture a range image, and its system is not particularlylimited. For example, the range sensor may adopt a time-of-flight systemusing a time of flight of light, or a passive system which calculates,by triangulation, depths of respective pixels from images captured by astereo camera. Assume that imaging parameters (a field angle,resolution, focal length, etc. at the time of imaging) of the rangesensor are given. The imaging parameters are used in the associationsearch unit 150 (to be described later).

The approximate value acquisition unit 120 acquires approximate valuesof the position and orientation of an object with respect to thethree-dimensional measurement apparatus 10. In this embodiment, theposition and orientation of the object with respect to thethree-dimensional measurement apparatus 10 express those of the objectwith reference to the image acquisition unit 110. However, the imageacquisition unit 110 need not always be used as a reference. Forexample, when the relative position and orientation of the object to acoordinate system of the image acquisition unit 110 are given, and theyremain unchanged, another portion of the three-dimensional measurementapparatus 10 may be used as a reference.

In this embodiment, as the approximate values of the position andorientation of the object, measurement values which were measured by thethree-dimensional measurement apparatus 10 previously (for example, atan immediately preceding timing) are used. However, the approximatevalues of the position and orientation of the object need not always besuch values. For example, time-series filtering processing (for example,a linear filter or Kalman filter) is applied to the previously measuredmeasurement values of the position and orientation of the object toestimate the motion velocity and angular velocity of the object. Then,values obtained by predicting the position and orientation of the objectbased on the estimation results may be used as the approximate values.Alternatively, the position and orientation of the object obtained froma sensor may be used as the approximate values. In this case, the sensorneed only measure the position and orientation of the object in sixdegrees of freedom, and its system (for example, a magnetic system,optical system, or ultrasonic system) is not particularly limited. Notethat when an approximate position and orientation where the object isplaced are determined in advance, these values may be used as theapproximate values. Furthermore, by applying object recognitionprocessing to a captured image or range image obtained by capturing animage of a visual scene including the object, the approximate values ofthe position and orientation of the object may be measured.

The model holding unit 130 holds three-dimensional geometric model data(to be also referred to as a three-dimensional shape model hereinafter)of a target object (to be also simply referred to as an objecthereinafter). The three-dimensional shape model can use the same modelas in the conventional method, and a detailed description thereof willnot be given. For example, the three-dimensional shape model is definedby pieces of information of planes configured by connecting point setsor points, and pieces of information of line segments which configureeach plane.

The geometric feature selection unit 140 selects points on arbitrarygeometric features from the three-dimensional shape model of the targetobject. In this embodiment, assume that the three-dimensional shapemodel is configured by a plurality of NURBS (Non-Uniform RationalB-Spline) curved surfaces, and geometric features of this model are setsof points and normals uniformly sampled from the respective NURBS curvedsurfaces. Of course, the present invention is not limited such specificgeometric features. For example, the geometric features of thethree-dimensional shape model may be the NURBS curved surfacesthemselves or meshes when the three-dimensional shape model is expressedby meshes.

The geometric feature selection unit 140 may perform the above selectionbased on information that prescribes geometric features which are easilymeasured from the three-dimensional shape model or measurement data andthose which are effective to calculate the position and orientation.That is, the geometric features can be those which can be used when theposition/orientation calculation unit 160 calculates the position andorientation of the target object. Since the geometric feature selectionunit 140 selects arbitrary geometric features, even when the positionand orientation are to be measured for an image which includes an objectother than the target object, association processing can be executedwithout increasing a calculation amount.

The geometric feature selection unit 140 selects points on geometricfeatures of the three-dimensional shape model based on the imagingparameters of the range sensor and the approximate values of theposition and orientation of the object acquired by the approximate valueacquisition unit 120. Based on them, only geometric features which canbe measured from a range image are selected.

More specifically, the three-dimensional shape model is rendered fromall directions, and geometric features which are viewable from therespective directions are registered in association with the directions.Then, the geometric feature selection unit 140 selects geometricfeatures registered in directions closest to a line of sight vector,which is calculated based on the approximate values of the position andorientation of the object, and the imaging parameters. Note that innerproducts between vectors of the respective directions and normals togeometric features may be compared, and only geometric features whosenormals oppose the direction vectors may be registered.

Note that the geometric feature selection unit 140 may select points ongeometric features based on a user instruction. That is, the user maymanually select points on geometric features with reference to a GUIthat displays the three-dimensional shape model of the target object.Alternatively, the center of a surface of each geometric feature may beselected as a point of that geometric feature of the three-dimensionalshape model. In addition, points detected by a feature detectordisclosed in A. E. Johnson and M. Hebert, “Efficient multiple modelrecognition in cluttered 3-d scenes,” Proc. Computer Vision and PatternRecognition, pp. 671-677, 1998. may be selected as points on geometricfeatures of the three-dimensional shape model. Furthermore, points whichare uniform on a range image may be selected based on the approximatevalues of the position and orientation of the object acquired by theapproximate value acquisition unit 120. More specifically, thethree-dimensional shape model is projected onto a two-dimensional imagebased on the approximate values of the position and orientation of theobject and the imaging parameters of the range sensor, and points on thethree-dimensional shape model, which are uniformly sampled on thetwo-dimensional image, are inversely projected onto a three-dimensionalspace. Then, points on geometric features, which become uniform on rangeimage, are calculated and selected. In addition, the selection method isnot particularly limited as long as points corresponding to geometricfeatures of a three-dimensional shape model are selected from thesurface of the model.

The association search unit 150 associates geometric features of thethree-dimensional shape model with those on a range image. Then, theassociation search unit 150 outputs the associated sets of the geometricfeatures of the three-dimensional shape model and those on the rangeimage.

As shown in FIG. 2, the association search unit 150 projects a point 240selected by the geometric feature selection unit 140 onto atwo-dimensional plane (projection plane) 220. Note that this projectionis performed based on the given imaging parameters (field angle,resolution, focal length, etc.) of a range image, and the approximatevalues of the position and orientation of the object acquired by theapproximate value acquisition unit 120. Formula (1) below is used toconvert three-dimensional coordinates (x, y, z) of a point selected bythe geometric feature selection unit 140 into coordinates (u, v) on thetwo-dimensional plane (projection plane) by perspective projection.

(u, v)^(T)=(f·x/z, f·y/z)^(T)  (1)

where f is a focal length.

Pixels on the projection plane and range image uniquely correspond toeach other, and a range 270 in the vicinity of the two-dimensionalcoordinates on the range image corresponding to the point (projectedpoint) projected onto the projection plane is searched for a geometricfeature to be associated on the range image. Since the projectioncalculation of a point can be attained by formula (1), an associationsearch can be a two-dimensional search around the projected point. Forthis reason, since the calculation amount can be reduced, associationprocessing between the geometric features of the three-dimensional shapemodel and those on a range image can be speeded up.

A geometric feature on a range image is searched for within a regionΩ330 formed by an elliptic region having the projected point as thecenter, as shown in FIG. 3. Note that the search range may be a pixel ofthe projected point or may have a rectangular shape having an arbitrarysize. A size and shape of the search region Ω330 may be decided based onat least any of, for example, the approximate values of the position andorientation of the object, the imaging parameters of the imageacquisition unit 110, and a motion of the object or image acquisitionunit 110. That is, the search range Ω330 may be a rectangular orelliptic region decided based on the line of sight direction of theimage acquisition unit 110 and the normal direction of a geometricfeature, or an elliptic region having a motion vector as a major axis.

In this embodiment, assume that a geometric feature on a range image isdefined by coordinates of a three-dimensional point obtained from depthinformation (distance information) included in a pixel corresponding tothe projected point. As a search method of a geometric feature on arange image, a measurement point within the search range Ω330 having ashortest distance from a projected point of a measurement point 340 canbe selected. Formula (2) is used to calculate the shortest distance ofcoordinates g_(i) on a range image of a measurement point measured at aprojected point p=(u, v)^(T).

argmin{∥g_(i)−p∥}, g_(i)∈Ω  (2)

Note that this search method need not always be adopted. For example,feature amounts of respective pixels within the search region Ω330 maybe calculated by a feature detector, and a feature amount most similarto that of a geometric feature of the three-dimensional shape model maybe selected. Alternatively, for example, a median value or average valueof coordinates on a three-dimensional space of measurement points withinthe search region Ω330, that is, coordinates on the space, which arecalculated by statistical processing may be selected. In addition, ageometric feature on a range image is not limited to a three-dimensionalpoint, but it may be parameters of a curved surface or plane, which arefitted to measurement points within the search region Ω, as long as thegeometric feature can be used when the position/orientation calculationunit 160 calculates the position and orientation of the target object.

Also, association error excluding processing may be executed. That is, anormal to a geometric feature selected on a range image is calculated,and the calculated normal is compared with that to a geometric featureof the three-dimensional shape model. If these normal directions aredifferent beyond a predetermined value, these geometric features are notassociated with each other. Note that the normal to the geometricfeature on the range image can be calculated by applying principalcomponent analysis to a three-dimensional point group adjacent to thegeometric feature, and deciding a third principal component as a normal.When the association error excluding processing is executed, errorswhich associate measurement data of different geometric features withthose of the three-dimensional shape model can be eliminated.

The position/orientation calculation unit 160 calculates the positionand orientation of the target object using the sets of the geometricfeatures of the three-dimensional shape model and those on the rangeimage, which are associated with each other by the association searchunit 150. In the calculations of the position and orientation of thetarget object in this embodiment, arithmetic processing is repeated tooptimize values obtained by the arithmetic processing. In this manner,differences of distances on the three-dimensional space between the setsof the geometric features of the three-dimensional shape model and thoseon the range image, which are associated with each other, are minimized.

In this embodiment, the geometric features of the three-dimensionalshape model are sets of coordinates on the three-dimensional space andpieces of normal information, and those on the range image arecoordinates of three-dimensional points. A geometric feature of thethree-dimensional shape model is considered as a small plane, and adistance between the plane on the three-dimensional shape model and athree-dimensional point calculated from a range image is used as adifference. Then, by minimizing this difference, the position andorientation of the target object are measured.

In this case, the difference depends on an expression method of ageometric feature. For example, if a geometric feature of thethree-dimensional shape model is a point, and that on a range image is apoint, the difference may be a distance between these points.Alternatively, for example, when a geometric feature of thethree-dimensional shape model is a plane and that on a range image is aplane, the difference may be a distance between these planes.Alternatively, for example, when a geometric feature of thethree-dimensional shape model is a plane and that on a range image is apoint, the difference may be a distance between the plane and point.Furthermore, the difference may be a distance which is approximatelycalculated using, for example, an implicit polynomial. That is, acalculation method is not particularly limited as long as thearrangement for measuring the position and orientation of the targetobject using an evaluation function based on differences of thedistances between the geometric features of the three-dimensional shapemodel and those on the range image is adopted.

An example of the operation in the three-dimensional measurementapparatus 10 shown in FIG. 1 will be described below using the flowchartshown in FIG. 4. In this case, the sequence of processing upon measuringthe position and orientation of an object will be explained.

The three-dimensional measurement apparatus 10 controls the imageacquisition unit 110 to capture a range image including depthinformation (distance information) from the range sensor. Thus, therange image is acquired (S110).

After completion of acquisition of the range image, thethree-dimensional measurement apparatus 10 controls the approximatevalue acquisition unit 120 to acquire the approximate values of theposition and orientation of the object with respect to thethree-dimensional measurement apparatus 10 (S120). As described above,the approximate values of the position and orientation of the object canuse, for example, the previously measured position and orientation,predicted values of the position and orientation by adding tinges ofestimation results of motions such as a velocity and angular velocity,measurement values by another sensor, and the object recognition resultsfrom a two-dimensional image and range image. Note that this embodimentuses the previously measured approximate values of the position andorientation as the approximate values of the position and orientation ofthe object. Note that the order of the processes in steps S110 and S120may be replaced.

Next, the three-dimensional measurement apparatus 10 startsposition/orientation measurement processing of the target object using,for example, the range image acquired by the process in step S110 andthe approximate values acquired by the process in step S120. Theposition/orientation measurement processing according to this embodimentuses a Levenberg-Marquardt method. More specifically, the approximatevalues of the position and orientation of the object are repetitivelycorrected by iterative arithmetic operations. In this way, thedifferences of the distances on the three-dimensional space between thegeometric features of the three-dimensional shape model and those on therange image are minimized, and values obtained as a result of thisprocessing are determined as the position and orientation of the targetobject. Of course, the present invention is not limited to theLevenberg-Marquardt method. For example, other optimization methods suchas a Gauss-Newton method and steepest descent method may be used.Alternatively, other nonlinear optimization calculation methods such asa conjugate gradient method may be used.

After the position/orientation measurement processing is started, thethree-dimensional measurement apparatus 10 acquires the approximatevalues of the target object obtained by the process in step S120 asinitial values of the position/orientation measurement processing. Then,the three-dimensional measurement apparatus 10 controls the geometricfeature selection unit 140 to select geometric features of thethree-dimensional shape model based on the approximate values (S130).

Upon completion of the selection, the three-dimensional measurementapparatus 10 controls the association search unit 150 to project thegeometric features of the three-dimensional shape model onto the rangeimage (two-dimensional plane) based on the approximate values of theposition and orientation of the target object and the imaging parameters(field angle, resolution, focal length, etc.) of the range sensor. Then,the association search unit 150 associates the geometric features of thethree-dimensional shape model with those on the range image, and outputsthe associated sets of the geometric features of the three-dimensionalshape model and those on the range image (S140). The three-dimensionalmeasurement apparatus 10 controls the position/orientation calculationunit 160 to update the approximate values of the position andorientation of the target object by the nonlinear optimization method(S150).

Finally, the three-dimensional measurement apparatus 10 controls theposition/orientation calculation unit 160 to make convergencedetermination. If calculated values are converged (YES in step S160),the processing ends; otherwise (NO in step S160), the process returns tostep S130, and the aforementioned processes in steps S130 to S150 arerepeated. Note that in the convergence determination, for example, ifdifferences between square sums of error vectors of the position andorientation before and after the update process are nearly “0”, it canbe determined that the calculated values are converged.

As described above, according to the first embodiment, upon associatingthe geometric features of the three-dimensional shape model and rangeimage with each other, points on the geometric features of thethree-dimensional shape model are selected, and the selected points onthe geometric features are projected onto the range image. Then, bysearching for geometric features which exist within regions ofpredetermined ranges from the projected points on the range image, thegeometric features of the three-dimensional shape model are associatedwith those on the range image. In this way, arithmetic operations can beefficiently executed upon associating the three-dimensional shape modelwith measurement data.

Second Embodiment

The second embodiment will be described below. The second embodimentwill explain a case in which a two-dimensional image is input as aninput image in addition to a range image. FIG. 5 is a view showing anoverview of processing for projecting geometric features of athree-dimensional shape model onto a two-dimensional plane, andsearching a two-dimensional image and range image for correspondinggeometric features.

For both of a range image 440 and two-dimensional image 450, geometricfeatures 410 and 420 of a three-dimensional shape model are projectedonto a two-dimensional plane (projection plane) 430 to search forcorresponding geometric features on the images from the two-dimensionalplane, thereby associating those geometric features with each other.

For this reason, the efficiency of arithmetic processing required toassociate the three-dimensional shape model and measurement data witheach other can be improved. Assume that in this embodiment, the rangeimage 440 and two-dimensional image 450 are captured at the sameviewpoint. In this case, the geometric features on the two-dimensionalimage can be relatively easily compared with those on the range image,thus allowing to determine a jump edge (isolated edge) in addition toelimination of association errors due to the influence of, for example,a shadow.

In this case, the overall arrangement of a three-dimensional measurementapparatus 10 according to the second embodiment is the same as that ofthe first embodiment, but processes in respective units are slightlydifferent. Such differences will be described below. Note that thefollowing description will be given while focusing on the differentprocesses.

An image acquisition unit 110 acquires a two-dimensional image and rangeimage. The range image is captured by a range sensor in the same manneras in the first embodiment. The two-dimensional image is captured by acamera (imaging apparatus) which is used to capture a normaltwo-dimensional image. The two-dimensional image to be captured may be adensity image or a color image. A camera model such as a focal lengthand principal point position of a camera and lens distortion parametersis calibrated in advance by a method disclosed in, for example, R. Y.Tsai, “A versatile camera calibration technique for high-accuracy 3Dmachine vision metrology using off-the-shelf TV cameras and lenses,”IEEE Journal of Robotics and Automation, vol. RA-3, no. 4, 1987.

In the second embodiment, for example, a range sensor of an activesystem, which is exemplified in the first embodiment, is used. This isbecause the range sensor of the active system, that using a stereocamera, and the like can also simultaneously capture a normaltwo-dimensional image. For this reason, the two-dimensional image andrange image can be captured from the same viewpoint. However, thetwo-dimensional image and range image need not always be captured fromthe same viewpoint. Note that when the imaging positions andorientations of imaging apparatuses in the two-dimensional image andrange image are closer to each other, and the geometrical relationshipbetween the two apparatuses is given, one of the two-dimensional imageand range image may undergo projection transformation based on thegeometrical relationship between the two apparatuses, and an imageobtained by that transformation may be used as that captured from thesame viewpoint.

An association search unit 150 associates the geometric features of thethree-dimensional shape model, which are selected by the geometricfeature selection unit 140, with those on the two-dimensional image andrange image, and outputs the associated sets of the geometric featuresof the three-dimensional shape model and those on the range image andtwo-dimensional image. The association search unit 150 attains theassociation processing by projecting the geometric features of thethree-dimensional shape model, which are selected by the geometricfeature selection unit 140, onto a two-dimensional plane (projectionplane), as in the first embodiment. Note that the association processingbetween the three-dimensional shape model and range image is the same asthat in the first embodiment. The association processing between thethree-dimensional shape model and two-dimensional image can use edges asgeometric features. Since the association processing based on the edgescan use the conventional technique, a detailed description thereof willnot be given. Note that the geometric features may be feature pointsdetected by a feature detector disclosed in C. Harris and M. Stephens,“A combined corner and edge detector,” Proc. 4th Alvey Vision Conf., pp.147-151, Manchester, U.K., 1988. In addition, the geometric features canbe those which can be used when a position/orientation calculation unit160 measures the position and orientation of the target object.

Since the two-dimensional image and range image according to thisembodiment are those captured from the same viewpoint, pixels on theprojection plane, two-dimensional image, and range image uniquelycorrespond to each other. For this reason, as in the first embodiment,using a predetermined range (a region) around a projected point as asearch region Ω, a geometric feature on an image corresponding to thatof the three-dimensional shape model is two-dimensionally searched for.The search region Ω may be a rectangular or elliptic region as in thefirst embodiment. Note that upon searching for edges on thetwo-dimensional image, a line having an arbitrary length may undergo alinear search in the normal direction of an edge.

Furthermore, association error excluding processing described in thefirst embodiment may be applied to the two-dimensional image. Theassociation error excluding processing is executed based on, forexample, comparison between feature amounts and that between edgedirections. If features which correspond to the edges or feature amountsdetected on the two-dimensional image cannot be detected from the rangeimage, the association error excluding processing may be executed toskip the association processing since it is determined that they are theedges or feature amounts caused by the influence of, for example, ashadow.

Also, processing for determining whether or not an edge detected on thetwo-dimensional image is a jump edge may be executed using the rangeimage. Since the range image and two-dimensional image are captured fromthe same viewpoint, the association error excluding processing can beefficiently executed using both the two-dimensional image and rangeimage.

The position/orientation calculation unit 160 calculates the positionand orientation of the target object using the sets of the geometricfeatures of the three-dimensional shape model and those on thetwo-dimensional image and range image, which are associated with eachother by the association search unit 150. Upon calculations of theposition and orientation of the target object in this embodiment,arithmetic processing is repeated to optimize the values obtained bythat processing. In this way, evaluation functions based on both ofdifferences of distances on a three-dimensional space between thegeometric features of the three-dimensional shape model and those on thetwo-dimensional space, and differences of distances on thethree-dimensional space between the geometric features of thethree-dimensional shape model and those on the range image areminimized.

Note that since the two-dimensional image does not include any depthinformation, the geometric features of the two-dimensional image areprojected onto the three-dimensional space in, for example, a statewithout any depth information. Of course, depth information may beassigned to each of the geometric features of the two-dimensional imageusing a predetermined algorithm or predetermined values.

In addition, the measurement method is not particularly limited as longas it measures the position and orientation of the target object basedon evaluation functions based on the differences between the geometricfeatures of the three-dimensional shape model and those on thetwo-dimensional image and range image. Note that the sequence ofposition/orientation measurement processing according to the secondembodiment is the same as that in the first embodiment, and adescription thereof will not be repeated.

Note that in the second embodiment, the geometric features to beassociated of the three-dimensional shape model may be curved surfaces,planes, or curves. However, in order to project complicated shapes suchas NURBS curved surfaces popularly used in a CAD model onto atwo-dimensional plane, tessellation or processing for clipping curvedsurfaces using contour lines is required, and a huge time is requiredfor arithmetic processing unless dedicated hardware for geometriccalculations such as a GPU is available. Hence, as in the firstembodiment, the geometric features of the three-dimensional shape modelare expressed as points or line segments, and the points or linesegments are projected onto the two-dimensional plane. With thisprojection, the association processing can be speeded up withoutrequiring any dedicated hardware.

As described above, according to the second embodiment, arithmeticoperations executed upon associating the three-dimensional shape modeland measurement data with each other can be efficiently made as in thefirst embodiment.

The representative embodiments of the present invention have beenexemplified. However, the present invention is not limited to theaforementioned and illustrated embodiments, and allows modifications asneeded without departing from the scope of its gist.

Note that the present invention can adopt embodiments in the form of,for example, a system, apparatus, method, program, or storage medium.More specifically, the present invention may be applied to either asystem configured by a plurality of devices or an apparatus including asingle device.

Other Embodiments

Aspects of the present invention can also be realized by a computer of asystem or apparatus (or devices such as a CPU or MPU) that reads out andexecutes a program recorded on a memory device to perform the functionsof the above-described embodiment(s), and by a method, the steps ofwhich are performed by a computer of a system or apparatus by, forexample, reading out and executing a program recorded on a memory deviceto perform the functions of the above-described embodiment(s). For thispurpose, the program is provided to the computer for example via anetwork or from a recording medium of various types serving as thememory device (for example, computer-readable storage medium).

While the present invention has been described with reference toexemplary embodiments, it is to be understood that the invention is notlimited to the disclosed exemplary embodiments. The scope of thefollowing claims is to be accorded the broadest interpretation so as toencompass all such modifications and equivalent structures andfunctions.

This application claims the benefit of Japanese Patent Application No.2010-039201 filed on Feb. 24, 2010, which is hereby incorporated byreference herein in its entirety.

1. A three-dimensional measurement apparatus comprising; imageacquisition means for acquiring a range image in which pixels representdistances from a target object; approximate value acquisition means foracquiring approximate values indicating a position and an orientation ofthe target object; selection means for selecting points corresponding togeometric features of a three-dimensional shape model of the targetobject; projection means for projecting a plurality of pointscorresponding to the geometric features selected by said selection meansonto the range image based on the approximate values indicating theposition and the orientation of the target object and imaging parametersat the time of imaging of the range image, search means for searchingregions of predetermined ranges respectively from the plurality ofprojected points for geometric features on the range image whichcorrespond to the geometric features of the three-dimensional shapemodel, and associating the geometric features with each other; andposition/orientation calculation means for calculating a position and anorientation of the target object using differences of distances on athree-dimensional space between the geometric features of thethree-dimensional shape model and the geometric features on the rangeimage, which are associated with each other by said search means.
 2. Theapparatus according to claim 1, characterized in that said imageacquisition means further acquires a two-dimensional image obtained bycapturing the target object, said projection means projects a pluralityof points corresponding to the geometric features selected by saidselection means onto the range image and the two-dimensional image basedon the approximate values indicating the position and the orientation ofthe target object and imaging parameters at the time of imaging of therange image, said search means searches regions of predetermined rangesrespectively from the plurality of projected points for geometricfeatures on the range image and the two-dimensional image whichcorrespond to the geometric features of the three-dimensional shapemodel, and associates the geometric features with each other, and saidposition/orientation calculation means calculates the position and theorientation of the target object using differences of distances on thethree-dimensional space between the geometric features of thethree-dimensional shape model and the geometric features on the rangeimage and the two-dimensional image, which are associated with eachother by said search means.
 3. The apparatus according to claim 1,characterized in that said search means compares a normal direction to ageometric feature of the three-dimensional shape model at a pointselected by said selection means with a normal direction to thegeometric feature detected from the predetermined range of the projectedpoint on the range image, and when the two normal directions aredifferent beyond a predetermined value, said search means skips theassociation processing of the geometric features.
 4. The apparatusaccording to claim 2, characterized in that the two-dimensional image iscaptured from the same viewpoint as the range image, and said searchmeans detects edges of the target object from the two-dimensional imageas the geometric features of the two-dimensional image, and skipsassociation processing between an edge which is not included in thegeometric features on the range image and a corresponding geometricfeature of the three-dimensional shape model upon execution of theassociation processing between the detected edges and the geometricfeatures of the three-dimensional shape model.
 5. The apparatusaccording to claim 1, characterized in that the geometric features ofthe three-dimensional shape model are expressed by points or linesegments.
 6. A processing method for a three-dimensional measurementapparatus for calculating a position and an orientation of a targetobject, characterized by comprising: a step of acquiring a range imagein which pixels represent distances from a target object; a step ofacquiring approximate values indicating a position and an orientation ofthe target object; a step of selecting points corresponding to geometricfeatures of a three-dimensional shape model of the target object; a stepof projecting a plurality of selected points corresponding to thegeometric features onto the range image based on the approximate valuesindicating the position and the orientation of the target object andimaging parameters at the time of imaging of the range image; a step ofsearching regions of predetermined ranges respectively from theplurality of projected points for geometric features on the range imagewhich correspond to the geometric features of the three-dimensionalshape model, and associating the geometric features with each other; anda step of calculating a position and an orientation of the target objectusing differences of distances on a three-dimensional space between thegeometric features of the three-dimensional shape model and thegeometric features on the range image, which are associated with eachother.
 7. A non-transitory computer-readable storage medium storing acomputer program for controlling a computer incorporated in athree-dimensional measurement apparatus for calculating a position andan orientation of a target object, to function as: image acquisitionmeans for acquiring a range image in which pixels represent distancesfrom a target object; approximate value acquisition means for acquiringapproximate values indicating a position and an orientation of thetarget object; selection means for selecting points corresponding togeometric features of a three-dimensional shape model of the targetobject; projection means for projecting a plurality of pointscorresponding to the geometric features selected by said selection meansonto the range image based on the approximate values indicating theposition and the orientation of the target object and imaging parametersat the time of imaging of the range image, search means for searchingregions of predetermined ranges respectively from the plurality ofprojected points for geometric features on the range image whichcorrespond to the geometric features of the three-dimensional shapemodel, and associating the geometric features with each other; andposition/orientation calculation means for calculating a position and anorientation of the target object using differences of distances on athree-dimensional space between the geometric features of thethree-dimensional shape model and the geometric features on the rangeimage, which are associated with each other by said search means.